package rescue;

import static java.awt.Color.BLUE;
import static java.awt.Color.CYAN;
import static java.awt.Color.GREEN;
import static java.awt.Color.MAGENTA;
import static java.awt.Color.ORANGE;
import static java.awt.Color.RED;
import static java.awt.Color.WHITE;
import static java.awt.Color.YELLOW;

import java.awt.Color;
import java.awt.image.BufferedImage;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;

import javax.vecmath.Vector3d;

import simbad.sim.RescueRobot;


public class MyRescueRobot extends RescueRobot
{
    private final double size = 10;
    private BufferedImage colourImage = null;
    private int imageWidth;
    private int imageHeight;

    // My attributes
    private final ArrayList<Color> OBJECT_COLORS = new ArrayList<Color>();
    private Color goalColor = null;
    private ObjectType goalType = null;
    private Behaviour currentGoal = Behaviour.SEARCH;
    private HashMap<Color, HashMap<Integer, ArrayList<Color>>> locations = new HashMap<Color, HashMap<Integer, ArrayList<Color>>>();
    
    // My constants
    private enum ObjectType {LANDMARK, OBSTACLE, VICTIM};
    private enum Behaviour {SEARCH, GOAL, AVOID, FINISH, SURVEY};
    private final int wallClearance = 2;
        
    public MyRescueRobot(Vector3d position, int id, String name)
    {
	super(position, id, name);
	colourImage = getCamera().createCompatibleImage();
	imageWidth  = colourImage.getWidth();
	imageHeight = colourImage.getHeight();
	
	OBJECT_COLORS.addAll(Arrays.asList(BLUE, CYAN, GREEN, MAGENTA, ORANGE, RED, WHITE, YELLOW));
    }

        
    /** Initialize Agent's Behavior */
    @Override
	public void initBehavior()
    {
	setTranslationalVelocity(0.5);
    }

	private ArrayList<Color> visited = new ArrayList<Color>();
   
	
    public void performRobotBehavior() {
	    processedImage = new Image();
		getCamera().copyVisionImage(colourImage);
		processImage();
		switch (currentGoal) {
		case SEARCH : {
			search();
		} break;
		case SURVEY : {
			survey();
		} break;
		case GOAL : {
			goal();
		} break;
		case AVOID : {
			
		} break; 
		case FINISH :{
			endExploration();
		}
		
		}
		//System.out.println(goalColor);
	
		// When you have found all other victims, you MUST call
		// endExploration();
    }
    Image processedImage;
    private void processImage()
    {
	for (int i = 0; i < imageHeight; i++) {
	    for (int j = 0; j < imageWidth; j++) {
		// Extract the 8-bit colour values from the pixel
		int c = colourImage.getRGB(j, i);
		int red = (c & 0x00ff0000) >> 16;
		int green = (c & 0x0000ff00) >> 8;
		int blue = c & 0x000000ff;

		/*
		  You have to change to code below to detect whatever you want
		  for your convenience you should be able to use the present colors from
		  java.awt.Color:

		  java.awt.Color.BLUE;
		  java.awt.Color.CYAN;
		  java.awt.Color.GREEN;
		  java.awt.Color.MAGENTA;
		  java.awt.Color.ORANGE;
		  java.awt.Color.RED;
		  java.awt.Color.WHITE;
		  java.awt.Color.YELLOW;
		*/
		
		float[] hsbvals = {0,0,0};
		Color.RGBtoHSB(red, green, blue, hsbvals);
		if (!(blue == red && blue == green && green == red)) {
			hsbvals[1] = 1;
			hsbvals[2] = 1;
		} else {
			hsbvals[0] = 0;
			hsbvals[1] = 0;
			hsbvals[2] = 0;
		}
		Color visibleColor = new Color(Color.HSBtoRGB(hsbvals[0], hsbvals[1], hsbvals[2]));
		//System.out.println(visibleColor);
		processedImage.add(j, i, visibleColor);
	    }
    }
    }

    /**
     * Your subclass MUST implement this method.
     * It should traverse the map made by the robot during its
     * exploration of the arena and print out directions for how to
     * find each victim.
     *
     * You must call the methods "forwardTo()", "turnRight()", "turnLeft()"
     * to do the actual printing. This is so we can guarantee that all the
     * directions conform to our standard.
     */
    public void giveDirections()
    {
	/* put your code here */
    }
    
    private void search() {
    	if (!processedImage.visibleObjects().isEmpty()) {
    		/*
    		if (processedImage.visibleObjects().contains(Color.CYAN) && processedImage.visibleObjects().size() > 1) {
    			goalType = ObjectType.VICTIM;
    			processedImage.visibleObjects.remove(CYAN);
    		}
    		*/
    		
	    	stopTurn();
	    	currentGoal = Behaviour.GOAL;
	    	//if (processedImage.visibleObjects().toArray()[0].equals(visited.get(0)))
	    	goalColor = (Color) processedImage.visibleObjects().toArray()[0];
    	} else {
    		stopForward();
    		startRightTurn();
    	}
    }
    
    private void goal() {
    	if (!processedImage.visibleObjects().contains(goalColor)) {
    		if (getBumpers().oneHasHit()) {
    			stopTurn();
    			currentGoal = Behaviour.SURVEY;
    			visited.add(goalColor);
    			
    			//System.out.println(visited);
    		
    		} else {
    			stopForward();
        		startLeftTurn();
    		}
    	} else {
    		stopTurn();
    		startForward();
    	}
    }
    
    private Color initialColor = null;
    private boolean rotating = false;
    private double turnTime = 0;
    private HashSet<Color> colorMap= new HashSet<Color>();
    private void survey() {
    	if (!rotating) {
    		stopForward();
    		startRightTurn();
    		turnTime = remainingTime();
    		rotating = true;
    	} else {
    		if (!processedImage.visibleObjects().isEmpty()) {
    			for (int i=0; i<processedImage.visibleObjects().size(); i++) {
    				if (initialColor == null) {
    					if (!processedImage.visibleObjects().toArray()[0].equals(goalColor)) {
    						initialColor = (Color) processedImage.visibleObjects().toArray()[0];
    						System.out.println("ic " + initialColor);
    					}
    				} else {
    					colorMap.add((Color) processedImage.visibleObjects().toArray()[i]);
    				}
    			}
    			if (remainingTime() <= turnTime - 10) {
					stopTurn();
					rotating = false;
					
					System.out.println(colorMap);
					
					colorMap.clear();
					currentGoal = Behaviour.SEARCH;
				}
    		}
    	}
    	/*
    	processedImage.visibleObjects().remove(goalColor);
    	if (initialColor != null) {
    		
    	} else {
    		if (!processedImage.visibleObjects().isEmpty()) {
    			for (Color c : processedImage.visibleObjects()) {
    				if (locations.get(goalColor) == null) {
    					locations.put(goalColor, new HashMap<Integer, ArrayList<Color>>());
    					locations.get(goalColor).put(0, new ArrayList<Color>());
    					locations.get(goalColor).get(0).add(c);
    					initialColor = c;
    				} else {
    					//locations.get(goalColor)
    				}
    			}
    		}
    	}
    	*/
    }
	////////////////////////////////////////////////////////////
	// OTHER CLASSES ///////////////////////////////////////////
	////////////////////////////////////////////////////////////
    
    private class Image extends ArrayList<ArrayList<Color>> {

		public Image() {
			super();
		}
		
		private HashSet<Color> visibleObjects = null;
		public HashSet<Color> visibleObjects() {
			if (visibleObjects == null) {
				visibleObjects = new HashSet<Color>();
				for (int x = this.columns() / 4; x < this.columns() - columns() / 4; x++) {
				//int x = this.columns() / 2;
					for (int y = 0; y<this.rows(); y++) {
						Color c = get(x,y);
						if (OBJECT_COLORS.contains(c) && !visibleObjects.contains(c)) {
							visibleObjects.add(c);
						}
					}
				}
				
				

				//for(int i=0; i< visibleObjects.size(); i++) {
				//	System.out.println(visibleObjects.get(i));	
				//}
			}
			
			return visibleObjects;
		}

		public void add(int x, int y, Color item) {
			ArrayList<Color> row;
			if (this.size() <= y) {
				add(y, new ArrayList<Color>());
			}
			if (item.getBlue() != item.getRed() || item.getBlue() != item.getGreen() || item.getGreen() != item.getRed()) {
				//System.out.println(item);
			}
			row = get(y);
			row.add(x, item);
		}
		
		public Color get(int x, int y) {
			return (get(y).get(x));
		}
		
		public int rows() {
			return size();
		}
		
		public int columns() {
			return get(0).size();
		}
    	
    }
    
}
